﻿
// 2023/11/15: 基于v6 2023/10/30版本。新增z轴相对速度、绝对速度、相对加速度、绝对加速度字段
// 2025/4/8： 修正color字段解析
// 2025/8/7: 新增ObjectClass.WheelBarrow, TruckHead, TrafficSign, TrafficLight

#ifndef OBJ_SENSOR_SAMPLE_V7_H
#define OBJ_SENSOR_SAMPLE_V7_H

#include "spadas.h"

namespace obj_sensor_sample_v7
{
	using namespace spadas;

	enum PositionMode
	{
		PM_ClosestPoint = 1, // 最近点
		PM_BoxCenter = 2, // 框中心
	};

	enum ObjectClass
	{
		OC_General = 1, // 一般物体大类
		OC_Car = 2, // 车辆大类
		OC_Ped = 3, // 行人大类
		OC_Truck = 4, // 货车大类
		OC_Bike = 5, // Bike大类
		OC_RailCar = 6, // 轨道车辆大类
		OC_Special = 7, // 特殊物体大类
		OC_RoadStatic = 8, // 道路内静态物体大类
		OC_SideStatic = 9, // 道路外静态物体大类

		OC_GeneralSmall = 11, // 一般小物体
		OC_GeneralBig = 12, // 一般大物体
		OC_Barrier = 13, // 一般障碍物

		OC_Van = 21, // 面包车
		OC_Minibus = 22, // 小巴
		OC_Bus = 23, // 大巴
		OC_BatteryCart = 24, // 园区电瓶车
		OC_TinyCar = 25, // 微型车
		OC_SUV = 26, // SUV

		OC_Adult = 31, // 成人
		OC_Child = 32, // 小孩
		OC_Scooter = 33, // 平衡车
		OC_WheelChair = 34, // 轮椅
		OC_WheelBarrow = 35, // 手推车

		OC_Minitruck = 41, // 小卡车
		OC_ContainerTruck = 42, // 货柜车（货柜车头 + 拖车）
		OC_SpecialCar = 43, // 特种车辆
		OC_Trailer = 44, // 拖车
		OC_TruckHead = 45, // 货柜车头

		OC_Motorbike = 51, // 摩托车
		OC_Bicycle = 52, // 自行车
		OC_ElectricBike = 53, // 电瓶自行车
		OC_Tricycle = 54, // 三轮车

		OC_Train = 61, // 火车
		OC_Tram = 62, // 有轨电车

		OC_Animal = 71, // 动物
		OC_Ball = 72, // 球类
		OC_Litter = 73, // 垃圾等杂物

		OC_Cone = 81, // 锥形路障
		OC_ManholeCover = 82, // 井盖
		OC_Patch = 83, // 路面补丁
		OC_Gantry = 84, // 龙门架

		OC_Pole = 91, // 竖杆
		OC_Tree = 92, // 树木
		OC_Vegetation = 93, // 灌木
		OC_Building = 94, // 建筑物
		OC_TrafficSign = 95, // 交通标志
		OC_TrafficLight = 96, // 交通灯
	};

	struct ObjectColor
	{
		Bool valid;
		Byte r; // Red component of the object's color / 目标物的颜色R分量
		Byte g; // Green component of the object's color / 目标物的颜色g分量
		Byte b; // Blue component of the object's color / 目标物的颜色b分量

		ObjectColor() : valid(FALSE), r(0), g(0), b(0)
		{}
	};

	struct ObjectInfo
	{
		// Tracking info / 跟踪信息
		Int id; // ID of the object / 目标物ID
		Int age; // Age of the object / 目标物的Age
		OptionalInt rawID; // Raw ID of the object / 目标物的原始ID
		OptionalInt rawAge; // Raw age of the object / 目标物的原始Age
		OptionalDouble confidence; // [%] Confidence (existence) of the object / 目标物的置信度

		// Time related / 时间偏差
		OptionalInt timeOffset; // [us] Time offset (to the frame's time) / 时间偏置(相对于帧的时间戳)

		// Color info / 颜色信息
		ObjectColor color; // Color of the object / 目标物的颜色

		// Class related / 类别信息
		OptionalInt rawClassID; // Raw classification ID of the object / 目标物的原始类别ID
		ObjectClass classs; // Classification of the object / 目标物的类别
		OptionalDouble classConfidence; // [%] Confidence (classification) of the object / 目标物的分类置信度

		// Position ("pos" differs by PositionMode, "cp" is always closest point) / 位置信息(pos根据PositionMode意义不同，cp专指ClosestPoint)
		PositionMode posMode; // Position mode of the object 目标物的位置模式
		Double posx; // [m] X-axis coordination of the object's position / 目标物的x轴方向位置
		Double posy; // [m] Y-axis coordination of the object's position / 目标物的y轴方向位置
		Double posz; // [m] Z-axis coordination of the object's position / 目标物的z轴方向位置
		OptionalDouble posxSigma; // [m] X-axis sigma of the object's position / 目标物x轴方向位置的精度
		OptionalDouble posySigma; // [m] Y-axis sigma of the object's position / 目标物y轴方向位置的精度
		OptionalDouble poszSigma; // [m] Z-axis sigma of the object's position / 目标物z轴方向位置的精度
		Double cpx; // [m] X-axis coordination of the object's closest point / 目标物的最近点x轴坐标
		Double cpy; // [m] Y-axis coordination of the object's closest point / 目标物的最近点y轴坐标
		Double cpd; // [m] Distance between the object's closest point and subject vehicle's contour / 目标物的最近点与本车轮廓距离

		// Size, heading, trajectory / 尺寸, 朝向, 轨迹
		OptionalDouble width; // [m] Width of the object's box / 目标物的宽度
		OptionalDouble length; // [m] Length of the object's box / 目标物的长度
		OptionalDouble height; // [m] Height of the object's box / 目标物的高度
		OptionalDouble heading; // [deg] Orientation of the object / 目标物的朝向
		Array<Point2D> trajectory; // List of trajectory points / 目标物轨迹点的列表

		// Velocity, acceleration / 速度, 加速度
		OptionalDouble vxRel; // [KPH] X-axis relative velocity of the object / x轴方向相对速度
		OptionalDouble vxAbs; // [KPH] X-axis absolute velocity of the object / x轴方向绝对速度
		OptionalDouble vyRel; // [KPH] Y-axis relative velocity of the object / y轴方向相对速度
		OptionalDouble vyAbs; // [KPH] Y-axis absolute velocity of the object / y轴方向绝对速度
		OptionalDouble vzRel; // [KPH] Z-axis relative velocity of the object / z轴方向相对速度
		OptionalDouble vzAbs; // [KPH] Z-axis absolute velocity of the object / z轴方向绝对速度
		OptionalDouble axRel; // [m/s²] X-axis relative acceleration of the object / x轴方向相对加速度
		OptionalDouble axAbs; // [m/s²] X-axis absolute acceleration of the object / x轴方向绝对加速度
		OptionalDouble ayRel; // [m/s²] Y-axis relative acceleration of the object / y轴方向相对加速度
		OptionalDouble ayAbs; // [m/s²] Y-axis absolute acceleration of the object / y轴方向绝对加速度
		OptionalDouble azRel; // [m/s²] Z-axis relative acceleration of the object / z轴方向相对加速度
		OptionalDouble azAbs; // [m/s²] Z-axis absolute acceleration of the object / z轴方向绝对加速度

		// Turning / 转向
		OptionalDouble yawRate; // [deg/s] Yaw rate of the object / 横摆角速度
		OptionalDouble curvature; // [1/m] Turning curvature of the object / 转弯曲率

		// Contour info / 轮廓信息
		Array<Point2D> contour; // List of contour points / 目标物轮廓点的列表

		// Others / 其他信息
		OptionalDouble rcs; // [dB] Radar-cross-section of the radar target / 雷达目标物的雷达散射截面
		OptionalDouble vLng; // [m/s] Longitudinal velocity of the radar target / 雷达目标物的径向速度
		OptionalDouble vLat; // [m/s] Lateral velocity of the radar target / 雷达目标物的切向速度

		ObjectInfo()
		{
			id = 0;
			age = 0;
			classs = OC_General;
			posMode = PM_ClosestPoint;
			posx = posy = posz = 0;
			cpx = cpy = cpd = 0;
		}

		Point2D position()
		{
			return Point2D((Float)posx, (Float)posy);
		}

		Point2D closestPoint()
		{
			return Point2D((Float)cpx, (Float)cpy);
		}

		OptionalDouble speed()
		{
			if (vxAbs.valid && vyAbs.valid)
			{
				if (vzAbs.valid) return math::root(math::square(vxAbs.value) + math::square(vyAbs.value) + math::square(vzAbs.value));
				else return math::root(math::square(vxAbs.value) + math::square(vyAbs.value));
			}
			else return OptionalDouble();
		}

		ObjectClass category()
		{
			if ((UInt)classs < 10) return classs;
			else return (ObjectClass)((UInt)classs / 10);
		}
	};

	struct ObjectSensorFov
	{
		Double positionX; // [m] X-axis coordination of the FOV's center point / FOV中心点x轴坐标
		Double positionY; // [m] Y-axis coordination of the FOV's center point / FOV中心的y轴坐标
		Double angleRange; // [deg] Angle range of the FOV / FOV的角度范围
		Double orientation; // [deg] Central heading angle of the FOV FOV中轴线朝向角
		Double distanceRange; // [m] Detect range of the FOV / FOV探测距离范围
		Double blindRange; // [m] Blind range of the FOV / FOV盲区范围

		ObjectSensorFov()
		{
			positionX = 0;
			positionY = 0;
			angleRange = 90;
			orientation = 0;
			distanceRange = 100;
			blindRange = 0;
		}
	};

	struct ObjectSensorSample
	{
		FullTimestamp timestamp; // 时间戳

		// Object info / 目标物信息
		Array<ObjectInfo> objects; // List of objects / 目标物列表
		OptionalInt koFrontIndex; // Index of CIPV object / 前向关键目标序号
		OptionalInt koLeftIndex; // Index of left side key object / 左侧关键目标序号
		OptionalInt koRightIndex; // Index of right side key object / 右侧关键目标序号

		// Subject vehicle info / 车辆参数部分
		OptionalDouble vehiVX; // [KPH] Subject vehicle's speed / 本车车速
		OptionalDouble vehiCurv; // [1/m] Subject vehicle's turning curvature / 本车曲率
		OptionalDouble vehiWidth; // [m] Subject vehicle's width / 本车宽度
		OptionalDouble vehiLength; // [m] Subject vehicle's length / 本车长度
		OptionalDouble vehiWheelBase; // [m] Subject vehicle's wheel base / 本车轴距
		OptionalDouble vehiFrontOverhang; // [m] Subject vehicle's front overhang / 本车前悬

		// FOV info / FOV信息
		Array<ObjectSensorFov> fovs; // List of FOV / FOV列表

		ObjectSensorSample()
		{}

		static String getProtocolName(UInt channel)
		{
			if (channel == UINF) return "obj-sensor-sample-v7";
			else return (String)"obj-sensor-sample-v7@" + channel;
		}

		static Bool supportInterpolation()
		{
			return FALSE;
		}

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp = timestamp;

			UInt trajectoryPoints = 0;
			UInt contourPoints = 0;
			Array<UInt> trajectoryOffset(objects.size());
			Array<UInt> trajectorySize(objects.size());
			Array<UInt> contourOffset(objects.size());
			Array<UInt> contourSize(objects.size());
			for (UInt i = 0; i < objects.size(); i++)
			{
				trajectoryOffset[i] = trajectoryPoints;
				trajectorySize[i] = objects[i].trajectory.size();
				trajectoryPoints += trajectorySize[i];
				contourOffset[i] = contourPoints;
				contourSize[i] = objects[i].contour.size();
				contourPoints += contourSize[i];
			}

			sample.values = Array<GeneralElement>(20 + objects.size() * 72 + fovs.size() * 6 + (trajectoryPoints + contourPoints) * 2);
			GeneralElement *data = sample.values.data();

			UInt objBase = 20;
			UInt fovBase = objBase + objects.size() * 72;
			UInt trajectoryBase = fovBase + fovs.size() * 6;
			UInt contourBase = trajectoryBase + trajectoryPoints * 2;

			data[0] = (Double)objects.size();
			data[1] = (Double)fovs.size();
			data[2] = (Double)trajectoryPoints;
			data[3] = (Double)contourPoints;

			data[4] = koFrontIndex.valid ? (Double)koFrontIndex.value : OptionalDouble();
			data[5] = koLeftIndex.valid ? (Double)koLeftIndex.value : OptionalDouble();
			data[6] = koRightIndex.valid ? (Double)koRightIndex.value : OptionalDouble();

			data[7] = vehiVX;
			data[8] = vehiCurv;
			data[9] = vehiWidth;
			data[10] = vehiLength;
			data[11] = vehiWheelBase;
			data[12] = vehiFrontOverhang;

			for (UInt i = 0; i < objects.size(); i++)
			{
				ObjectInfo& src = objects[i];
				UInt baseIndex = objBase + 72 * i;
				data[baseIndex + 0] = (Double)src.id;
				data[baseIndex + 1] = (Double)src.age;
				data[baseIndex + 2] = src.rawID.valid ? (Double)src.rawID.value : OptionalDouble();
				data[baseIndex + 3] = src.rawAge.valid ? (Double)src.rawAge.value : OptionalDouble();
				data[baseIndex + 4] = src.rawClassID.valid ? (Double)src.rawClassID.value : OptionalDouble();
				data[baseIndex + 5] = (UInt)src.classs;
				data[baseIndex + 6] = (UInt)src.posMode;
				data[baseIndex + 7] = src.posx;
				data[baseIndex + 8] = src.posy;
				data[baseIndex + 9] = src.posz;
				data[baseIndex + 10] = src.posxSigma;
				data[baseIndex + 11] = src.posySigma;
				data[baseIndex + 12] = src.poszSigma;
				data[baseIndex + 13] = src.cpx;
				data[baseIndex + 14] = src.cpy;
				data[baseIndex + 15] = src.cpd;
				data[baseIndex + 16] = src.width;
				data[baseIndex + 17] = src.length;
				data[baseIndex + 18] = src.height;
				data[baseIndex + 19] = src.heading;
				data[baseIndex + 20] = src.vxRel;
				data[baseIndex + 21] = src.vxAbs;
				data[baseIndex + 22] = src.vyRel;
				data[baseIndex + 23] = src.vyAbs;
				data[baseIndex + 24] = src.vzRel;
				data[baseIndex + 25] = src.vzAbs;
				data[baseIndex + 26] = src.axRel;
				data[baseIndex + 27] = src.axAbs;
				data[baseIndex + 28] = src.ayRel;
				data[baseIndex + 29] = src.ayAbs;
				data[baseIndex + 30] = src.azRel;
				data[baseIndex + 31] = src.azAbs;
				data[baseIndex + 32] = src.color.valid ? (Double)src.color.r : OptionalDouble();
				data[baseIndex + 33] = src.color.valid ? (Double)src.color.g : OptionalDouble();
				data[baseIndex + 34] = src.color.valid ? (Double)src.color.b : OptionalDouble();
				data[baseIndex + 35] = src.timeOffset.valid ? (Double)src.timeOffset.value : OptionalDouble();
				data[baseIndex + 36] = src.confidence;
				data[baseIndex + 37] = src.classConfidence;
				data[baseIndex + 38] = getTTC1(src);
				data[baseIndex + 39] = getTTC2(src);
				data[baseIndex + 40] = getTHW(src);
				data[baseIndex + 41] = getDCA(src, 1.0);
				data[baseIndex + 42] = getDCA(src, 0.0);
				data[baseIndex + 43] = getOverlap(src);
				data[baseIndex + 44] = getLC(src);
				data[baseIndex + 45] = getRC(src);
				data[baseIndex + 46] = src.rcs;
				data[baseIndex + 47] = src.vLng;
				data[baseIndex + 48] = src.vLat;
				data[baseIndex + 49] = src.yawRate;
				data[baseIndex + 50] = src.curvature;
				data[baseIndex + 51] = src.speed();
				data[baseIndex + 68] = (Double)trajectoryOffset[i];
				data[baseIndex + 69] = (Double)trajectorySize[i];
				data[baseIndex + 70] = (Double)contourOffset[i];
				data[baseIndex + 71] = (Double)contourSize[i];


				UInt dataOffset = trajectoryBase + trajectoryOffset[i] * 2;
				for (UInt n = 0; n < trajectorySize[i]; n++)
				{
					Point2D& pt = src.trajectory[n];
					data[dataOffset + 2 * n] = pt.x;
					data[dataOffset + 2 * n + 1] = pt.y;
				}

				dataOffset = contourBase + contourOffset[i] * 2;
				for (UInt n = 0; n < contourSize[i]; n++)
				{
					Point2D& pt = src.contour[n];
					data[dataOffset + 2 * n] = pt.x;
					data[dataOffset + 2 * n + 1] = pt.y;
				}
			}

			for (UInt i = 0; i < fovs.size(); i++)
			{
				ObjectSensorFov& src = fovs[i];
				UInt baseIndex = fovBase + 6 * i;
				data[baseIndex + 0] = src.positionX;
				data[baseIndex + 1] = src.positionY;
				data[baseIndex + 2] = src.orientation;
				data[baseIndex + 3] = src.angleRange;
				data[baseIndex + 4] = src.distanceRange;
				data[baseIndex + 5] = src.blindRange;
			}

			sample.significantCount = trajectoryBase;
			return sample;
		}

		Bool fromGeneralSample(String protocol, SessionGeneralSample sample)
		{
			timestamp = sample.timestamp;

			if (protocol == "obj-sensor-sample-v7")
			{
				Array<GeneralElement> v = sample.values;
				if (v.size() < 20) return FALSE;

				if (!v[0].valid ||
					!v[1].valid ||
					!v[2].valid ||
					!v[3].valid) return FALSE;

				Int nObjects = (Int)v[0].value;
				Int nFovs = (Int)v[1].value;
				Int nTrajectories = (Int)v[2].value;
				Int nContours = (Int)v[3].value;

				Int sizeWithExtra = 20 + nObjects * 72 + nFovs * 6 + (nTrajectories + nContours) * 2;
				Int sizeWithoutExtra = 20 + nObjects * 72 + nFovs * 6;
				if ((Int)v.size() != sizeWithExtra && (Int)v.size() != sizeWithoutExtra) return false;

				koFrontIndex = v[4].valid ? (Int)v[4].value : OptionalInt();
				koLeftIndex = v[5].valid ? (Int)v[5].value : OptionalInt();
				koRightIndex = v[6].valid ? (Int)v[6].value : OptionalInt();

				vehiVX = v[7];
				vehiCurv = v[8];
				vehiWidth = v[9];
				vehiLength = v[10];
				vehiWheelBase = v[11];
				vehiFrontOverhang = v[12];

				Int objBase = 20;
				Int fovBase = objBase + 72 * nObjects;
				Int trajBase = fovBase + 6 * nFovs;
				Int contBase = trajBase + 2 * nTrajectories;

				objects = Array<ObjectInfo>(nObjects);
				for (Int i = 0; i < nObjects; i++)
				{
					ObjectInfo& obj = objects[i];
					UInt baseIndex = objBase + 72 * i;

					obj.id = (Int)v[baseIndex + 0].value;
					obj.age = (Int)v[baseIndex + 1].value;
					obj.rawID = v[baseIndex + 2].valid ? (Int)v[baseIndex + 2].value : OptionalInt();
					obj.rawAge = v[baseIndex + 3].valid ? (Int)v[baseIndex + 3].value : OptionalInt();
					obj.rawClassID = v[baseIndex + 4].valid ? (Int)v[baseIndex + 4].value : OptionalInt();
					obj.classs = (ObjectClass)(Int)v[baseIndex + 5].value;
					obj.posMode = (PositionMode)(Int)v[baseIndex + 6].value;
					obj.posx = v[baseIndex + 7].value;
					obj.posy = v[baseIndex + 8].value;
					obj.posz = v[baseIndex + 9].value;
					obj.posxSigma = v[baseIndex + 10];
					obj.posySigma = v[baseIndex + 11];
					obj.poszSigma = v[baseIndex + 12];
					obj.cpx = v[baseIndex + 13].value;
					obj.cpy = v[baseIndex + 14].value;
					obj.cpd = v[baseIndex + 15].value;
					obj.width = v[baseIndex + 16];
					obj.length = v[baseIndex + 17];
					obj.height = v[baseIndex + 18];
					obj.heading = v[baseIndex + 19];
					obj.vxRel = v[baseIndex + 20];
					obj.vxAbs = v[baseIndex + 21];
					obj.vyRel = v[baseIndex + 22];
					obj.vyAbs = v[baseIndex + 23];
					obj.vzRel = v[baseIndex + 24];
					obj.vzAbs = v[baseIndex + 25];
					obj.axRel = v[baseIndex + 26];
					obj.axAbs = v[baseIndex + 27];
					obj.ayRel = v[baseIndex + 28];
					obj.ayAbs = v[baseIndex + 29];
					obj.azRel = v[baseIndex + 30];
					obj.azAbs = v[baseIndex + 31];
					obj.color.r = v[baseIndex + 32].valid ? (Byte)v[baseIndex + 32].value : 0;
					obj.color.g = v[baseIndex + 33].valid ? (Byte)v[baseIndex + 33].value : 0;
					obj.color.b = v[baseIndex + 34].valid ? (Byte)v[baseIndex + 34].value : 0;
					obj.color.valid = v[baseIndex + 32].valid && v[baseIndex + 33].valid && v[baseIndex + 34].valid;
					obj.timeOffset = v[baseIndex + 35].valid ? (Int)v[baseIndex + 35].value : OptionalInt();
					obj.confidence = v[baseIndex + 36];
					obj.classConfidence = v[baseIndex + 37];
					obj.rcs = v[baseIndex + 46];
					obj.vLng = v[baseIndex + 47];
					obj.vLat = v[baseIndex + 48];
					obj.yawRate = v[baseIndex + 49];
					obj.curvature = v[baseIndex + 50];

					Int trajOffset = (Int)v[baseIndex + 68].value;
					Int trajSize = (Int)v[baseIndex + 69].value;
					Int contOffset = (Int)v[baseIndex + 70].value;
					Int contSize = (Int)v[baseIndex + 71].value;

					if ((Int)v.size() == sizeWithExtra)
					{
						obj.trajectory = Array<Point2D>(trajSize);
						for (int n = 0; n < trajSize; n++)
						{
							obj.trajectory[n].x = (Float)v[trajBase + 2 * trajOffset + 2 * n].value;
							obj.trajectory[n].y = (Float)v[trajBase + 2 * trajOffset + 2 * n + 1].value;
						}

						obj.contour = Array<Point2D>(contSize);
						for (int n = 0; n < contSize; n++)
						{
							obj.contour[n].x = (Float)v[contBase + 2 * contOffset + 2 * n].value;
							obj.contour[n].y = (Float)v[contBase + 2 * contOffset + 2 * n + 1].value;
						}
					}
					else
					{
						obj.trajectory = Array<Point2D>();
						obj.contour = Array<Point2D>();
					}
				}

				fovs = Array<ObjectSensorFov>(nFovs);
				for (int i = 0; i < nFovs; i++)
				{
					ObjectSensorFov& fov = fovs[i];
					Int baseIndex = fovBase + 6 * i;

					fov.positionX = v[baseIndex].value;
					fov.positionY = v[baseIndex + 1].value;
					fov.orientation = v[baseIndex + 2].value;
					fov.angleRange = v[baseIndex + 3].value;
					fov.distanceRange = v[baseIndex + 4].value;
					fov.blindRange = v[baseIndex + 5].value;
				}

				return TRUE;
			}
			else if (protocol == "obj-sensor-sample-v6")
			{
				Array<GeneralElement> v = sample.values;
				if (v.size() < 16) return FALSE;

				if (!v[0].valid ||
					!v[1].valid ||
					!v[2].valid ||
					!v[3].valid) return FALSE;

				Int nObjects = (Int)v[0].value;
				Int nFovs = (Int)v[1].value;
				Int nTrajectories = (Int)v[2].value;
				Int nContours = (Int)v[3].value;

				Int sizeWithExtra = 16 + nObjects * 52 + nFovs * 6 + (nTrajectories + nContours) * 2;
				Int sizeWithoutExtra = 16 + nObjects * 52 + nFovs * 6;
				if ((Int)v.size() != sizeWithExtra && (Int)v.size() != sizeWithoutExtra) return false;

				koFrontIndex = v[4].valid ? (Int)v[4].value : OptionalInt();
				koLeftIndex = v[5].valid ? (Int)v[5].value : OptionalInt();
				koRightIndex = v[6].valid ? (Int)v[6].value : OptionalInt();

				vehiVX = v[7];
				vehiCurv = v[8];
				vehiWidth = v[9];
				vehiLength = v[10];
				vehiWheelBase = v[11];
				vehiFrontOverhang = v[12];

				Int objBase = 16;
				Int fovBase = objBase + 52 * nObjects;
				Int trajBase = fovBase + 6 * nFovs;
				Int contBase = trajBase + 2 * nTrajectories;

				objects = Array<ObjectInfo>(nObjects);
				for (Int i = 0; i < nObjects; i++)
				{
					ObjectInfo& obj = objects[i];
					UInt baseIndex = objBase + 52 * i;

					obj.id = (Int)v[baseIndex + 0].value;
					obj.age = (Int)v[baseIndex + 1].value;
					obj.rawID = v[baseIndex + 2].valid ? (Int)v[baseIndex + 2].value : OptionalInt();
					obj.rawAge = v[baseIndex + 3].valid ? (Int)v[baseIndex + 3].value : OptionalInt();
					obj.rawClassID = v[baseIndex + 4].valid ? (Int)v[baseIndex + 4].value : OptionalInt();
					obj.classs = (ObjectClass)(Int)v[baseIndex + 5].value;
					obj.posMode = (PositionMode)(Int)v[baseIndex + 6].value;
					obj.posx = v[baseIndex + 7].value;
					obj.posy = v[baseIndex + 8].value;
					obj.cpx = v[baseIndex + 9].value;
					obj.cpy = v[baseIndex + 10].value;
					obj.cpd = v[baseIndex + 11].value;
					obj.width = v[baseIndex + 12];
					obj.length = v[baseIndex + 13];
					obj.heading = v[baseIndex + 14];
					obj.vxRel = v[baseIndex + 15];
					obj.vxAbs = v[baseIndex + 16];
					obj.vyRel = v[baseIndex + 17];
					obj.vyAbs = v[baseIndex + 18];
					obj.axRel = v[baseIndex + 19];
					obj.axAbs = v[baseIndex + 20];
					obj.ayRel = v[baseIndex + 21];
					obj.ayAbs = v[baseIndex + 22];
					obj.color.r = v[baseIndex + 23].valid ? (Byte)v[baseIndex + 23].value : 0;
					obj.color.g = v[baseIndex + 24].valid ? (Byte)v[baseIndex + 24].value : 0;
					obj.color.b = v[baseIndex + 25].valid ? (Byte)v[baseIndex + 25].value : 0;
					obj.color.valid = v[baseIndex + 23].valid && v[baseIndex + 24].valid && v[baseIndex + 25].valid;
					obj.timeOffset = v[baseIndex + 26].valid ? (Int)v[baseIndex + 26].value : OptionalInt();
					obj.confidence = v[baseIndex + 27];
					obj.classConfidence = v[baseIndex + 28];
					obj.height = v[baseIndex + 29];
					obj.posz = v[baseIndex + 30].value;
					obj.posxSigma = v[baseIndex + 31];
					obj.posySigma = v[baseIndex + 32];
					obj.poszSigma = v[baseIndex + 33];
					obj.rcs = v[baseIndex + 42];
					obj.vLng = v[baseIndex + 43];
					obj.vLat = v[baseIndex + 44];
					obj.yawRate = v[baseIndex + 45];
					obj.curvature = v[baseIndex + 46];

					Int trajOffset = (Int)v[baseIndex + 48].value;
					Int trajSize = (Int)v[baseIndex + 49].value;
					Int contOffset = (Int)v[baseIndex + 50].value;
					Int contSize = (Int)v[baseIndex + 51].value;

					if ((Int)v.size() == sizeWithExtra)
					{
						obj.trajectory = Array<Point2D>(trajSize);
						for (int n = 0; n < trajSize; n++)
						{
							obj.trajectory[n].x = (Float)v[trajBase + 2 * trajOffset + 2 * n].value;
							obj.trajectory[n].y = (Float)v[trajBase + 2 * trajOffset + 2 * n + 1].value;
						}

						obj.contour = Array<Point2D>(contSize);
						for (int n = 0; n < contSize; n++)
						{
							obj.contour[n].x = (Float)v[contBase + 2 * contOffset + 2 * n].value;
							obj.contour[n].y = (Float)v[contBase + 2 * contOffset + 2 * n + 1].value;
						}
					}
					else
					{
						obj.trajectory = Array<Point2D>();
						obj.contour = Array<Point2D>();
					}
				}

				fovs = Array<ObjectSensorFov>(nFovs);
				for (int i = 0; i < nFovs; i++)
				{
					ObjectSensorFov& fov = fovs[i];
					Int baseIndex = fovBase + 6 * i;

					fov.positionX = v[baseIndex].value;
					fov.positionY = v[baseIndex + 1].value;
					fov.orientation = v[baseIndex + 2].value;
					fov.angleRange = v[baseIndex + 3].value;
					fov.distanceRange = v[baseIndex + 4].value;
					fov.blindRange = v[baseIndex + 5].value;
				}

				return TRUE;
			}
			else if (protocol == "obj-sensor-sample-v5")
			{
				Array<GeneralElement> v = sample.values;
				if (v.size() < 11) return FALSE;

				if (!v[0].valid ||
					!v[1].valid ||
					!v[2].valid ||
					!v[10].valid) return FALSE;

				Int nObjects = (Int)v[0].value;
				Int nTrajectories = (Int)v[1].value;
				Int nContours = (Int)v[2].value;
				Int nFovs = (Int)v[10].value;

				Int sizeWithExtra = 11 + nObjects * 42 + nFovs * 6 + (nTrajectories + nContours) * 2;
				Int sizeWithoutExtra = 11 + nObjects * 42 + nFovs * 6;
				if ((Int)v.size() != sizeWithExtra && (Int)v.size() != sizeWithoutExtra) return false;

				koFrontIndex = v[3].valid ? (Int)v[3].value : OptionalInt();
				koLeftIndex = v[4].valid ? (Int)v[4].value : OptionalInt();
				koRightIndex = v[5].valid ? (Int)v[5].value : OptionalInt();

				vehiVX = v[6];
				vehiCurv = v[7];
				vehiWidth = v[8];
				vehiLength = v[9];

				Int objBase = 11;
				Int fovBase = objBase + 42 * nObjects;
				Int trajBase = fovBase + 6 * nFovs;
				Int contBase = trajBase + 2 * nTrajectories;

				objects = Array<ObjectInfo>(nObjects);
				for (Int i = 0; i < nObjects; i++)
				{
					ObjectInfo& obj = objects[i];
					UInt baseIndex = objBase + 42 * i;

					obj.id = (Int)v[baseIndex + 0].value;
					obj.age = (Int)v[baseIndex + 1].value;
					obj.rawID = v[baseIndex + 2].valid ? (Int)v[baseIndex + 2].value : OptionalInt();
					obj.rawAge = v[baseIndex + 3].valid ? (Int)v[baseIndex + 3].value : OptionalInt();
					obj.rawClassID = v[baseIndex + 4].valid ? (Int)v[baseIndex + 4].value : OptionalInt();
					obj.classs = (ObjectClass)(Int)v[baseIndex + 5].value;
					obj.posMode = (PositionMode)(Int)v[baseIndex + 6].value;
					obj.posx = v[baseIndex + 7].value;
					obj.posy = v[baseIndex + 8].value;
					obj.cpx = v[baseIndex + 9].value;
					obj.cpy = v[baseIndex + 10].value;
					obj.cpd = v[baseIndex + 11].value;
					obj.width = v[baseIndex + 12];
					obj.length = v[baseIndex + 13];
					obj.heading = v[baseIndex + 14];
					obj.vxRel = v[baseIndex + 15];
					obj.vxAbs = v[baseIndex + 16];
					obj.vyRel = v[baseIndex + 17];
					obj.vyAbs = v[baseIndex + 18];
					obj.axRel = v[baseIndex + 19];
					obj.axAbs = v[baseIndex + 20];
					obj.ayRel = v[baseIndex + 21];
					obj.ayAbs = v[baseIndex + 22];
					obj.timeOffset = v[baseIndex + 35].valid ? (Int)v[baseIndex + 35].value : OptionalInt();
					obj.color.r = v[baseIndex + 36].valid ? (Byte)v[baseIndex + 36].value : 0;
					obj.color.g = v[baseIndex + 37].valid ? (Byte)v[baseIndex + 37].value : 0;
					obj.color.b = v[baseIndex + 38].valid ? (Byte)v[baseIndex + 38].value : 0;
					obj.color.valid = v[baseIndex + 36].valid && v[baseIndex + 37].valid && v[baseIndex + 38].valid;
					obj.classConfidence = v[baseIndex + 39];
					obj.posxSigma = v[baseIndex + 40];
					obj.posySigma = v[baseIndex + 41];

					Int trajOffset = (Int)v[baseIndex + 31].value;
					Int trajSize = (Int)v[baseIndex + 32].value;
					Int contOffset = (Int)v[baseIndex + 33].value;
					Int contSize = (Int)v[baseIndex + 34].value;

					if ((Int)v.size() == sizeWithExtra)
					{
						obj.trajectory = Array<Point2D>(trajSize);
						for (int n = 0; n < trajSize; n++)
						{
							obj.trajectory[n].x = (Float)v[trajBase + 2 * trajOffset + 2 * n].value;
							obj.trajectory[n].y = (Float)v[trajBase + 2 * trajOffset + 2 * n + 1].value;
						}

						obj.contour = Array<Point2D>(contSize);
						for (int n = 0; n < contSize; n++)
						{
							obj.contour[n].x = (Float)v[contBase + 2 * contOffset + 2 * n].value;
							obj.contour[n].y = (Float)v[contBase + 2 * contOffset + 2 * n + 1].value;
						}
					}
					else
					{
						obj.trajectory = Array<Point2D>();
						obj.contour = Array<Point2D>();
					}
				}

				fovs = Array<ObjectSensorFov>(nFovs);
				for (int i = 0; i < nFovs; i++)
				{
					ObjectSensorFov& fov = fovs[i];
					Int baseIndex = fovBase + 6 * i;

					fov.positionX = v[baseIndex].value;
					fov.positionY = v[baseIndex + 1].value;
					fov.orientation = v[baseIndex + 2].value;
					fov.angleRange = v[baseIndex + 3].value;
					fov.distanceRange = v[baseIndex + 4].value;
					fov.blindRange = v[baseIndex + 5].value;
				}

				return TRUE;
			}
			else return FALSE;
		}

		static ObjectSensorSample interpolate(ObjectSensorSample& s1, Double w1, ObjectSensorSample& s2, Double w2, FullTimestamp timestamp)
		{
			return ObjectSensorSample();
		}

		static Array<Point2D> calBoxCorners(ObjectInfo& obj, Double k) // 8 points are connected sequentially, and k is the reduction ratio of inclined points (generally 0.9~1) / 8个点顺序连接, k为斜方向点缩小比例(一般为0.9~1)
		{
			Double posx = obj.posx;
			Double posy = obj.posy;
			Double width = obj.width.value;
			Double length = obj.length.value;
			Double heading = obj.heading.valid ? obj.heading.value : 0;
			Float coef = (Float)k;

			Array<Point2D> pts(8);
			pts[0].x = (Float)(length / 2); pts[0].y = 0;
			pts[1].x = (Float)(length / 2) * coef; pts[1].y = (Float)(width / 2) * coef;
			pts[2].x = 0; pts[2].y = (Float)(width / 2);
			pts[3].x = (Float)(-length / 2) * coef; pts[3].y = (Float)(width / 2) * coef;
			pts[4].x = (Float)(-length / 2); pts[4].y = 0;
			pts[5].x = (Float)(-length / 2) * coef; pts[5].y = (Float)(-width / 2) * coef;
			pts[6].x = 0; pts[6].y = (Float)(-width / 2);
			pts[7].x = (Float)(length / 2) * coef; pts[7].y = (Float)(-width / 2) * coef;

			Double coshd = math::cos(heading);
			Double sinhd = math::sin(heading);
			for (UInt i = 0; i < 8; i++)
			{
				Point2D& pt = pts[i];
				Double x = coshd * pt.x - sinhd * pt.y;
				Double y = sinhd * pt.x + coshd * pt.y;
				pt.x = (Float)(posx + x);
				pt.y = (Float)(posy + y);
			}

			return pts;
		}

		OptionalDouble getTTC1(ObjectInfo& obj) // Calculate first order TTC / 计算一阶TTC [s]
		{
			if (obj.vxRel.valid)
			{
				double vxRelM = obj.vxRel.value / 3.6;

				if (obj.cpx > 0)
				{
					if (vxRelM >= -0.1) return 20;
					else return math::min(20.0, -(obj.cpx / vxRelM));
				}
				else if (vehiLength.valid && obj.cpx < -vehiLength.value)
				{
					if (vxRelM <= 0.1) return 20;
					else return math::min(20.0, (-vehiLength.value - obj.cpx) / vxRelM);
				}
				else return OptionalDouble();
			}
			else return OptionalDouble();
		}

		OptionalDouble getTTC2(ObjectInfo& obj) // Calculate second order TTC / 计算二阶TTC [s]
		{
			if (obj.vxRel.valid && obj.axRel.valid)
			{
				double vxRelM = obj.vxRel.value / 3.6;
				double axRel = obj.axRel.value;

				if (obj.cpx > 0)
				{
					if (vxRelM >= -0.1) return 20;
					if (math::abs(axRel) < 0.1) return math::min(20.0, -(obj.cpx / vxRelM));
					if (vxRelM * vxRelM - 2 * axRel * obj.cpx >= 0) return math::min(20.0, (-vxRelM - math::root(vxRelM * vxRelM - 2 * axRel * obj.cpx)) / axRel); // 0.5at^2+vt-d=0
					return 20;
				}
				else if (vehiLength.valid && obj.cpx < -vehiLength.value)
				{
					Double dist = -vehiLength.value - obj.cpx;

					if (vxRelM <= 0.1) return 20;
					else if (math::abs(axRel) < 0.1) return math::min(20.0, (-vehiLength.value - obj.cpx) / vxRelM);
					else if (vxRelM * vxRelM + 2 * axRel * dist >= 0) return math::min(20.0, (-vxRelM + math::root(vxRelM * vxRelM + 2 * axRel * dist)) / axRel);
					return 20;
				}
				else return OptionalDouble();
			}
			else return OptionalDouble();
		}

		OptionalDouble getTHW(ObjectInfo& obj) // Calculate time headway / 计算车间时距 [s]
		{
			if (obj.cpx > 0 && vehiVX.valid)
			{
				double vxEgoM = vehiVX.value / 3.6;
				if (vxEgoM <= 0.1) return 20;
				else return math::min(20.0, obj.cpx / vxEgoM);
			}
			else return OptionalDouble();
		}

		OptionalDouble getDCA(ObjectInfo& obj, Double reactionTime = 0) // Calculate DCA / 计算避免碰撞最小减速度 [m/s²] reactionTime=0: ODCA, >0: PDCA
		{
			if (obj.cpx > 0 && vehiVX.valid && obj.vxAbs.valid && obj.axAbs.valid)
			{
				Double vxEgoM = vehiVX.value / 3.6;
				Double vxAbsM = obj.vxAbs.value / 3.6;
				Double axAbs = obj.axAbs.value;

				Double egoDxReaction = vxEgoM * reactionTime;
				Double objDxReaction = vxAbsM * reactionTime + 0.5 * axAbs * reactionTime * reactionTime;

				Double dxRelAR = obj.cpx - 0.1 + objDxReaction - egoDxReaction;
				Double egoVxAbsAR = vxEgoM;
				Double objVxAbsAR = vxAbsM + axAbs * reactionTime;
				Double objVxRelAR = vxAbsM - vxEgoM;
				Double objAxAbsAR = axAbs;

				if (dxRelAR <= 0) return -20;

				if (objAxAbsAR >= 0.0)
				{
					if (objVxRelAR >= 0.0) return 0;
					else return math::max(-20.0, objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR));
				}
				else
				{
					if (objVxRelAR >= 0.0) return math::max(-20.0, egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR));
					if (objVxRelAR * objVxRelAR / (2 * dxRelAR) < objAxAbsAR) return math::max(-20.0, egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR));
					else return math::max(-20.0, objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR));
				}
			}
			else return OptionalDouble();
		}

		OptionalDouble getOverlap(ObjectInfo& obj, Double boxCornerRatio = 0.98) // Calculate overlap / 计算重叠率 [%]
		{
			if (vehiCurv.valid && vehiWidth.valid)
			{
				Double widthEgo = vehiWidth.value;
				Double curvEgo = vehiCurv.value;
				Double fo = vehiFrontOverhang.valid ? vehiFrontOverhang.value : 0.9;
				Double wb = vehiWheelBase.valid ? vehiWheelBase.value : 2.8;

				if (obj.posx <= 0 || widthEgo <= 0) return OptionalDouble();

				Double widthObj;
				if (obj.posMode == PM_ClosestPoint)
				{
					switch (obj.category())
					{
					case OC_Car:
						widthObj = 1.9;
						break;
					case OC_Ped:
						widthObj = 0.5;
						break;
					case OC_Truck:
						widthObj = 2.4;
						break;
					case OC_Bike:
						widthObj = 0.9;
						break;
					case OC_RailCar:
						widthObj = 2.6;
						break;
					default:
						widthObj = 1.0;
						break;
					}
				}
				else // BoxCenter
				{
					Array<Point2D> corners = calBoxCorners(obj, boxCornerRatio);
					Double miny = DINF, maxy = NDINF;
					for (UInt i = 0; i < corners.size(); i++)
					{
						miny = math::min(miny, (Double)corners[i].y);
						maxy = math::max(maxy, (Double)corners[i].y);
					}
					widthObj = maxy - miny;
				}

				Double dyEgo = 0.5 * curvEgo * obj.posx * obj.posx + curvEgo * (fo + wb) * obj.posx;
				Double dyObj = obj.posy;
				Double overLap = (math::min(dyEgo + 0.5 * widthEgo, dyObj + 0.5 * widthObj) - math::max(dyEgo - 0.5 * widthEgo, dyObj - 0.5 * widthObj));
				Double overLapRate = overLap / (overLap >= 0 ? math::min(widthEgo, widthObj) : widthEgo);
				return (int)(math::max(-3.0, overLapRate) * 100.0);
			}
			else return OptionalDouble();
		}

		OptionalDouble getLC(ObjectInfo& obj, Double boxCornerRatio = 0.98) // Calculate lateral clearance / 计算横向间距 [m]
		{
			if (vehiWidth.valid)
			{
				if (obj.posMode == PM_ClosestPoint)
				{
					if (obj.posx > 0) return OptionalDouble();
					else if (obj.posy > 0) return obj.posy - vehiWidth.value * 0.5;
					else return -vehiWidth.value * 0.5 - obj.posy;
				}
				else // PM_BoxCenter
				{
					Array<Point2D> points = calBoxCorners(obj, boxCornerRatio);

					Double minx = DINF;
					Double miny = DINF, maxy = NDINF;
					for (UInt i = 0; i < points.size(); i++)
					{
						Point2D& pt = points[i];
						if (pt.x < minx) minx = pt.x;
						if (pt.y < miny) miny = pt.y;
						if (pt.y > maxy) maxy = pt.y;
					}

					if (minx > 0) return OptionalDouble();
					else if (miny > 0) return miny - vehiWidth.value * 0.5;
					else if (maxy < 0) return -vehiWidth.value * 0.5 - maxy;
					else return OptionalDouble();
				}
			}
			else return OptionalDouble();
		}

		OptionalDouble getRC(ObjectInfo& obj, Double boxCornerRatio = 0.98) // Calculate rear clearance / 计算后向间距 [m]
		{
			if (vehiLength.valid)
			{
				Array<Point2D> points;
				if (obj.posMode == PM_ClosestPoint) points = Array<Point2D>::scalar(Point2D((Float)obj.posx, (Float)obj.posy));
				else points = calBoxCorners(obj, boxCornerRatio);

				Double maxx = NDINF;
				for (UInt i = 0; i < points.size(); i++)
				{
					Point2D& pt = points[i];
					if (pt.x > maxx) maxx = pt.x;
				}

				if (maxx > 0) return OptionalDouble();
				else return -vehiLength.value - maxx;
			}
			else return OptionalDouble();
		}
	};
}

#endif
